/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_MotionFrame_H_
#define __MMM_MotionFrame_H_

#include "../../MMMCore.h"
#include "../../MMMImportExport.h"
#include "MotionFrameEntries.h"

#include <Eigen/Core>
#include <string> 
#include <vector>
#include <map>

namespace MMM
{

/*!
    \brief MotionFrame describes a snapshot of the motion at a timestep.
*/

class MotionFrame;
typedef boost::shared_ptr<MotionFrame> MotionFramePtr;

class MMM_IMPORT_EXPORT MotionFrame
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MotionFrame(unsigned int ndof);

    /*!
      Copy Constructor
    */
    MotionFrame(const MotionFrame &inpSrc);

    MotionFramePtr copy(){
        MotionFramePtr res(new MotionFrame(this->getndof()));
        res->joint = this->joint;
        res->joint_acc = this->joint_acc;
        res->joint_vel = this->joint_vel;
        res->setRootPose(this->getRootPose());
        res->timestep = this->timestep;
        return res;
    }

	/*!
		Returns the root pose as homogeneous 4x4 matrix.
		Position is given in millimeters.
	*/
	Eigen::Matrix4f getRootPose();

	/*!
        Returns standard entry "RootPosition".
        This is the 3D position of the model's root coordinate system in millimeters.
	*/
	Eigen::Vector3f getRootPos();
	
    /*!
        Returns standard entry "RootPositionVelocity".
        This is the velocity of the model's root coordinate system.
	*/
	Eigen::Vector3f getRootPosVel();

    /*!
        Returns standard entry "RootPositionAcceleration"
        This is the acceleration of the model's root coordinate system.
	*/
	Eigen::Vector3f getRootPosAcc();

	/*!
        Returns standard entry "RootRotation".
        This is the orientation of the model's root coordinate system.
		Specified in Roll Pitch Yaw angles.
	*/
	Eigen::Vector3f getRootRot();

    /*!
        Returns standard entry "RootRotationVelocity".
        This is the rotation velocity of the model's root coordinate system.
	*/
	Eigen::Vector3f getRootRotVel();

	/*!
        Returns standard entry "RootRotationAcceleration"
        This is the rotation acceleration of the model's root coordinate system.
	*/
	Eigen::Vector3f getRootRotAcc();

	/*!
		Sets the root pose as homogeneous 4x4 matrix.
		Position is given in millimeters.
	*/
	bool setRootPose(const Eigen::Matrix4f &pose);

	/*!
        Sets the standard entry "RootPosition".
		This is the 3D position of the model's root coordinate system in millimeters.
	*/
	bool setRootPos(const Eigen::Vector3f &pos);

	/*!
        Sets the standard entry "RootPositionVelocity".
		This is the velocity of the model's root coordinate system.
	*/
	bool setRootPosVel(const Eigen::Vector3f &posVel);

	/*!
    Sets the standard entry "RootPositionAcceleration"
	This is the acceleration of the model's root coordinate system.
	*/
	bool setRootPosAcc(const Eigen::Vector3f &posAcc);

	/*!
    Sets the standard entry "RootRotation".
	This is the orientation of the model's root coordinate system.
	Specified in Roll Pitch Yaw angles.
	*/
	bool setRootRot(const Eigen::Vector3f &rot);

	/*!
    Sets the standard entry "RootRotationVelocity".
	This is the rotation velocity of the model's root coordinate system.
	*/
	bool setRootRotVel(const Eigen::Vector3f &rotVel);

	/*!
    Sets the standard entry "RootRotationAcceleration"
	This is the rotation acceleration of the model's root coordinate system.
	*/
	bool setRootRotAcc(const Eigen::Vector3f &rotAcc);



    //! The timestep (s)
	float timestep;

    //! The joint values
	Eigen::VectorXf joint;

    //! The joint velocities (optional)
	Eigen::VectorXf joint_vel;

    //! The joint accelerations (optional)
    Eigen::VectorXf joint_acc;

    /** Custom Matrix for debugging purposes only
     * DO NOT USE! Will be removed in future versions!
     *
     */
    Eigen::MatrixXf _customMatrix;

	/*! 
		Adds a custom motion data entry.  
        Silently overwrites existing entries with same name (lowercase).
        \param name The name is used to identify the custom data. Internally it is converted to lowercase.
        \param entry The data to store.
        \return True on success.
	*/
    bool addEntry(const std::string &name, MotionFrameEntryPtr entry );

	//! Remove custom entry 
	bool removeEntry(const std::string &name);

	//! Check if entry with name (lowercase) is present.
	bool hasEntry(const std::string &name) const;

	//! Retrieve custom motion data entries. Name is internally converted to lowercase.
    MotionFrameEntryPtr getEntry(const std::string &name);

    //! Returns an XML string that describes this object.
	std::string toXML();

    unsigned int getndof() {return ndof;}

protected:
	//! Here, custom motion data fields can be stored
    std::map<std::string, MotionFrameEntryPtr> motionFrameEntries;

    MotionFrame(){} // do not allow construction without parameter

    //! The number of Degrees of Freedom.
	unsigned int ndof;



};

}

#endif 
